//Copyright 2010 Neurosciences Research Foundation, Incorporated

// world class encapsulates the world.
// world::interact() allows you to ineract with the robot/environment.
// Ideally, all communicating with the robot would be hidden inside this object
//  s.t. if a particular processor needs to get/set some value, it will be 
//  available from this object, even if from a romote host.
//
// For now, we can have each node simulate an arm redundantly.


#include "main.h"
#include <iostream>
#include <iomanip>
#include <fstream>
#include <string>
#include <sstream>
#include <algorithm>
#include <vector>
#include <map>
#include <mpi.h>
#include "motormap.h"
#include "groups.h"

#ifdef SIMULATED_ROBOT
#define APEX2
#else
#define APEX3
#endif // SIMULATED_ROBOT


#ifdef APEX1
#include "robot_real.h"
#endif

#ifdef APEX2
#include "robot_sim.h"
#endif

#ifdef APEX3
#include "apex_robot.h"
#endif

using namespace std;

extern float x[][3];

//Rotation Station Definitions
unsigned int rot_cntr = 0;
float rot_poses[] = { 0, 75, 150, 225, 300 };

class electrode{
public:
	float position[3];
	int start_msec;
	int stop_msec;
	float dist;
	float stim_current;
	
};

//*******************************************
float dist(float x[3], float y[3], int n=3){

	float d = 0.0f;
	for(int i = 0; i < n; i++){
		d+=(x[i]-y[i])*(x[i]-y[i]);
	}
	return sqrt(d);
}

class world 
{
  private:
	  vector<float> mfr;
	  vector<electrode> electrodes;
	
  public:

  #ifdef APEX3
	  typedef ::nsi::thinklink::scoped_ptr< ::nsi::thinklink::device > device;
  #endif

    void init(group igroup_data[MAX_AREAS][MAX_TYPES]) 
    {	
		  sim_end_time_sec = 17000;

		  // initialization ...
		  group_data.load_groups(igroup_data);
		
		  max_neuron_id = group_data.get_max_neuron_id();
			   		   
		  mfr.resize(max_neuron_id+1,0.0);
		
		  //*************************************
		  // Set up the motor control system.
		  // Assume that each spot in the layer V pyramidal cell array
		  // has a specific muscle synergy that is position based.
		  // 
		
		  group g = group_data["M1p5(L56)"];

		  vector<float> one_row;
		  one_row.resize(g.cols,0.0f);
		
		  shoulder_lut_rad.resize(g.rows,one_row);
		  deltoid_lut_rad.resize(g.rows,one_row);
		  elbow_lut_rad.resize(g.rows,one_row);
		
		  float prow = 0.0f;
		  for( int row = 0; row< g.rows; row++)
      {
			  float pcol = 0.0f;
			  for( int col = 0; col < g.cols; col++ )
        {
				  shoulder_lut_rad[row][col] = motorprimitives[(int) prow*25+(int)pcol][0] * range_rad[0];
				  deltoid_lut_rad[row][col]  = motorprimitives[(int) prow*25+(int)pcol][1] * range_rad[1];
				  elbow_lut_rad[row][col]    = motorprimitives[(int) prow*25+(int)pcol][2] * range_rad[2];		
				  pcol=pcol+24.999/g.cols;
			  }
			  prow=prow+24.999/g.rows;
		  }
		  fprintf(stderr, "Finished loading world\n");  
	}

	~world() 
  {
		std::cout << "----------  Quitting now. Goodbye.  ----------" << std::endl;
    
		if( myprocid == 0 )
    {
			//apex.quit();
			fout.close();
			patternhist.close();
		}
	}
	
#ifdef APEX3
	void reset()
	{
		apex_client.reset_link();
	}	
#endif

	void set_procid(int myid){		
		  myprocid = myid;
	}		  
	  
	  
#ifdef APEX3		
	void init_robot(int myid, ::nsi::thinklink::device* link)
  {
#else
	void init_robot(int myid)
  {		
#endif
		
		myprocid = myid;

#ifdef APEX3
		std::cout << "Executing init_robot() . . . " << std::endl;
		if( myprocid == 0 )
		{
			std::string gait_file = "LV_New_Walk_Concat4.bin";
			apex_client.init(gait_file, link);
      apex_client.configure_rotation_station(false);//configure rotation station w/ position control
		
    //*** APE-X COMMAND EXAMPLES ***
		/*
      //** APE-X Direct Commands
      apex_client.stand_up();
			apex_client.side_step(SIDE_STEP_LEFT);
			apex_client.side_step(SIDE_STEP_RIGHT);
			apex_client.walk_forward(2);
			apex_client.turn(TURN_RIGHT, 90);
			apex_client.sit_down();
    */

		/*
      //** APE-X Keyboard Control **
      char kb_ch = '\0';
			while( kb_ch != 'q' )
			{
				apex_client.keyboard_control(kb_ch);
				//apex_client.explore();	
				kb_ch = getch();		
			}
    */
      
    /*
      //** APE-X Rotation Station Control **
      int num_poses = 5;
      int trials = 10;
      int curr_pose = 0;
      //chosen object rotation angles in degrees
      float rot_poses[] = { 0, 75, 150, 225, 300 };             
      for( int i = 0; i < trials; ++i )
      {
        curr_pose = i%num_poses;
        std::cout << "Moving to rotation station angle: " <<  rot_poses[curr_pose] << std::endl;
        //command_rotation_station( float pos, int speed, int torque )
        apex_client.command_rotation_station( rot_poses[curr_pose], 800, 1000 ); 
        usleep(1500000);  
      }
    */

		}
#else
		apex.init(myid);
#endif
		
		if( myprocid == 0 )fout.open("motorcommands.dat");
		if( myprocid == 0 )patternhist.open("patternhist.dat");

		float shoulder = 0.0;
		float deltoid = 0.0;
		float elbow = 0.0;
		// Put the arm somewhere begin with.
		//apex.set_arm_command( shoulder, deltoid, elbow, 1.0f,1.0f,1.0f,0, 1, 0);

		
		
		// Debug:
		if( myprocid==0)
    {
			group g = group_data["M1p5(L56)"];
			int i = g.first_neuron_id;

			for( int row = 0; row< g.rows; row++)
      {
				for( int col = 0; col < g.cols; col++ )
        {
					cout << " " << i << " " << shoulder_lut_rad[row][col]<< " " << deltoid_lut_rad[row][col]<< " " << elbow_lut_rad[row][col]<<endl;
					i++;
				}
			}
		}
		
	};
	
	int myrand(int low, int hi){
		
		return low + rand()%(hi-low);	
		
	}

	
	float to01(float x,float minx,float maxx){
		
		return (x- minx)/(maxx-minx);
		
	}
	
	// Returns muscle length as a function of joint angle in radians, given the 
	// length of the tendon insertion points from the joint.
	float length(float a, float b, float theta_rad){
	
		return sqrt(a*a+b*b-2*a*b*cos(theta_rad));
		
	}
	
	// Returns derivative of the muscle length with respect to the joint angle (radians),
	// given the length of the tendon insertion points from the joint.
	// This can be used to convert angular joint velocity in radians per second to muscle 
	// length changes in linear units by calculating dtheta*dlength(theta).
	float dlength(float a, float b, float theta_rad){
		return a*b*sin(theta_rad)/sqrt(a*a+b*b-2*a*b*cos(theta_rad));
	}	
	
	
	
	
	
	

//***************************************************************************************************
	void stim_group(string group_name, vector<electrode> electrodes, int t_msec, int trial_duration_msec, float exc_output[]){
		
		group g;
		if( group_data.exists(group_name) ){
			g = group_data[group_name];
			int gnum=g.last_neuron_id-g.first_neuron_id;
			
			
			int i = g.first_neuron_id;
			for( int row = 0; row< g.rows; row++){
				for( int col = 0; col < g.cols; col++ ){
					for( int elect=0; elect < electrodes.size(); elect ++ ){
						if( t_msec%trial_duration_msec >= electrodes[elect].start_msec && t_msec%trial_duration_msec <= electrodes[elect].stop_msec){
							if( dist(x[i], electrodes[elect].position) < electrodes[elect].dist ){
								exc_output[i] = electrodes[elect].stim_current;
							}
						}			
					}							
					i++;
				}
			}
		}
		
	}	
	
		
		
		
//***************************************************************************************************
// Assumptions: outarray is a a 1d array pixels by pixels long that holds the gabor function output.
// Sx and Sy is the center of the gabor; theta is the angle, cycles is the number of cycles; all units in radians
// phase_percent is 0 to 1 and shifts the phase.
		void gabor(float* outarray, int pixels,float Sx,float Sy,float theta, float cycles, float phase_percent, float scale)
		{
			
			float xphase = phase_percent*cycles*pi*cos(theta);
			float yphase = phase_percent*cycles*pi*sin(theta);
			
			float step = 2*pi/((pixels-1)/cycles);
			
			int c=0;
			for (float x = (-cycles*pi+xphase);x<=(cycles*pi+xphase);x+=step){
				int r=0;
				for (float y = (-cycles*pi+yphase);y<=(cycles*pi+yphase);y+=step){
					
					float xPrime = x * cos(theta) + y * sin(theta);
					float yPrime = y * cos(theta) - x * sin(theta);
					outarray[c+r*pixels] = scale*exp(-0.5*((xPrime/Sx)*(xPrime/Sx)+(yPrime/Sy)*(yPrime/Sy)))*cos(xPrime);
					//cout <<" " << outarray[c+r*pixels] << endl;
					r=r+1;
				}
				c=c+1;
			}
		}
		
		
	
// This function allows you to reset neural areas depending on events in the task or environment.  This sample function demonstrates several methods for doing so.  Obviously this function could get every bit as complex as interact()	  
	  void reset_interact(int t) {
		  
		  if (t%pattern_duration_msec == 0)
		  {
			  // OLD VERSION, reset all neurons
			  reset_dynamics( );			  
			  
			  // NEW VERSION, still resets all neurons, but demonstrates how to do it by each group
			  // map<string,group> gmap=group_data.get_map(); 
			  // for ( map<string,group>::iterator it = gmap.begin(); it!=gmap.end(); it++ ) 
			  // { 
				//  if (myid == 0) 
				//	  cerr << "Resetting group" << (*it).first << " (" << (*it).second.first_neuron_id << "," << (*it).second.last_neuron_id <<") at time " << t << endl;  
				  
				//  reset_dynamics( (*it).second ); 
			  // } 
			  
			  // MORE USEFUL NEW VERSION
			  // if( group_data.exists("V1p23"))
				//  reset_dynamics( group_data["V1p23"] );
			  
			  // if( group_data.exists("V1b23"))
				 // reset_dynamics( group_data["V1b23"] );

			  // etc....
		  }
		  
	  }
	  
	
//*******************************	
// Get the spiked array so we can keep track of spikes. 
// Get a pointer to the external current array array so we can stimulate neurons.
// 	set exc_output[neuron] to add exc_output[neuron] to a given neuron number.  (Don't worry if the neuron
// 	is on this processor or not; we will take care of that in the simulator.)
// int t in msec.
// 
	
	void interact(int t, bool spiked[], float exc_output[], float m1_input[], float s1_input[])
	{
    static float last_shoulder,last_deltoid,last_elbow, last_sdot, last_ddot, last_edot; 
		float shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque;
		int sec = t/1000;
		int gnum,pnum;
		int kk,jj;
		
		int n_patterns = 25;
		float xo[]={.2, .6, 1, 1.4, 1.8,.2, .6, 1, 1.4, 1.8,.2, .6, 1, 1.4, 1.8,.2, .6, 1, 1.4, 1.8,.2, .6, 1, 1.4, 1.8};
		float yo[]={.2, .2, .2, .2, .2, .6, .6, .6, .6, .6,1,1,1,1,1, 1.4,1.4,1.4,1.4,1.4, 1.8,1.8,1.8,1.8,1.8};
		//int permu[]={7, 15, 9, 10, 5, 20, 8, 13, 17, 3, 16, 22, 21, 23, 4, 12, 6, 24, 2, 25, 1, 18, 19, 11, 14};


		
		int trial_duration_msec = 1000*n_patterns;
		
		int S1_stimulation_start_time_sec=0;
		int S1_stimulation_end_time_sec=sim_end_time_sec-00;  // Never stop this; when we restart from checkpoint, keep S1 stimulation.    
		
		int M1_stimulation_start_time_sec=0;
		//int M1_stimulation_end_time_sec=sim_end_time_sec-500; //30;   // Next, turn off layer 2/3 stimulation and see if reentry from S1 can activate layer 2.
		int M1_stimulation_end_time_sec=13000; 
		//int M1_stimulation_end_time_sec=2300; 
		
		// change back to sim_end_time_sec before checkin for the real robot
		int V1_stimulation_start_time_sec=14000;
		//int V1_stimulation_end_time_sec=sim_end_time_sec-300;   // Next, turn off layer 2/3 stimulation and see if reentry from S1 can activate layer 2.
		int V1_stimulation_end_time_sec=14000;
		//int V1_stimulation_end_time_sec=3000;
		//int V1_stimulation_end_time_sec=sim_end_time_sec;
		
		//int M1_out_stimulation_start_time_sec=sim_end_time_sec-200;
		int M1_out_stimulation_start_time_sec=14000+100;
		//int M1_out_stimulation_start_time_sec=3500;

		//int M1_out_stimulation_start_time_sec=1300000;
		int M1_out_stimulation_end_time_sec=sim_end_time_sec-0; // First, turn off motor layer stimulation and see if layer 2/3 still activates layer 5.

		// output pattern data
		pattern_duration_msec = trial_duration_msec/n_patterns;
		if( t%pattern_duration_msec == 0 ){
			patternhist << t << " " << (t%trial_duration_msec) / pattern_duration_msec << endl;
		}
		
		int steps=11;
		
		int n_electrodes = steps*2*4;

		if (sec>=0)
		{
			  
      int kill_sim_sec = 600;
      if( sec == kill_sim_sec ){
        exit(0);
      }

			const int motor_update_interval_ms = 250;
			const int motor_update_offset_ms = 100;
			
			#ifdef APEX3
      if(myid == 0)
      {
			  //apex_client.get_arm_proprioception(shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque, t);
        const int rotation_window_ms = 1000;
        int curr_pose = 0;   
        int num_poses = 2;   
        float max_degrees = 300.0;
        float pose_matrix[] = { 0.0, 300.0 };
			  
        //Let's use the rotation station for now . . .
        //Move the object once every second
        if( (t%rotation_window_ms) == 0 )
        {
          curr_pose = rot_cntr%num_poses;
          //apex_client.command_rotation_station( (float)(curr_pose*(max_degrees/(float)(num_poses-1))), 100, 500 );  
          apex_client.set_joint_rotation_station( pose_matrix[curr_pose], 1023);  
          ++rot_cntr;
        }
      }
			#else			
			  apex.get_arm_proprioception(shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque, t);
			#endif
			
			//sdot=(shoulder-last_shoulder)/0.25;
			//ddot=(deltoid-last_deltoid)/0.25;
			//edot=(elbow-last_elbow)/0.25;
			
			//cout << "(" << shoulder << " " << deltoid << " " << elbow << ")" << endl;

			/*
			
			if( t% motor_update_interval_ms == motor_update_offset_ms ){
						
				//shoulder=(float)rand()/(float)RAND_MAX*pi/2;
				//deltoid=(float)rand()/(float)RAND_MAX*pi/2;
				//elbow=(float)rand()/(float)RAND_MAX*pi/2;

				shoulder=s1_input[0];
				deltoid=s1_input[1];
				elbow=s1_input[2];
				sdot=(shoulder-last_shoulder)/0.25;
				ddot=(deltoid-last_deltoid)/0.25;
				edot=(elbow-last_elbow)/0.25;

				last_shoulder=shoulder;
				last_deltoid=deltoid;
				last_elbow=elbow;
				last_sdot = sdot;
				last_ddot = ddot;
				last_edot = edot;
			}
			else{
				shoulder = last_shoulder;
				deltoid = last_deltoid;
				elbow = last_elbow;
				sdot = last_sdot;
				ddot = last_ddot;
				edot = last_edot;
			}
			
			*/
			
			
			sdot=0.0;ddot=0.0;edot=0.0;
			
			
			float shoulder_sensory = shoulder;
			float deltoid_sensory = deltoid; 
			float elbow_sensory = elbow;			
			
			//apex.set_arm_command( shoulder, deltoid, elbow, sdot, ddot, edot,t, motor_update_interval_ms, motor_update_offset_ms);
			
			//apex.get_arm_proprioception(shoulder,deltoid,elbow, sdot,ddot,edot, storque, dtorque, etorque, t);

			//cout << shoulder << " --   " << sdot << endl;

			// convert to appropriate cell types from paper on spinal cord.
			const float a=1.0,b=.28;  // Muscle tendon insertion points (normalized).
			float temp, temp2;
			group g;

			
			// Try stimulating the intralaminar nucleus to simulate the awake state. McKinstry.  6-15-09.  This caused TCns to become stuck on.  Bummer.
			/*
			if( sec >= 1 ){
				if( group_data.exists("M1TCn")){
					g = group_data["M1TCn"];
					for(int gnum=g.first_neuron_id;gnum<g.last_neuron_id;gnum++){
						exc_output[gnum] = 2.0f;
					}
				}
			}
			*/
			
			float stim_current = 8000.0;
			float distance = 0.25;
			int interval=pattern_duration_msec;
			
			if( sec >= M1_stimulation_start_time_sec && sec <= M1_stimulation_end_time_sec && group_data.exists("M1p5(L56)") ){

				g = group_data["M1p5(L56)"];

				float stim_current = 2000.0;
				float distance = 0.2;

				float x= g.x0;
				float y= g.y0;
				float width_mm = g.area_width_mm;
				vector<electrode> electrodes(n_patterns);

				
				for(kk=0;kk<n_patterns;kk++){
					//electrodes[kk].position[0]=x+xo[permu[kk]-1]; 
					//electrodes[kk].position[1]=y+yo[permu[kk]-1]; 
					electrodes[kk].position[0]=x+xo[kk]; 
					electrodes[kk].position[1]=y+yo[kk]; 
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=kk*interval;
					electrodes[kk].stop_msec=(kk+1)*interval-50;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				}
				
				
				/*
				kk=0;
				electrodes[kk].position[0]=x+1.35;  //1.68;
				electrodes[kk].position[1]=y+1.54;   //1.72;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval;
				electrodes[kk].stop_msec=(kk+1)*interval-50;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				
				electrodes[kk].position[0]=x+0.66;  //     0.56;
				electrodes[kk].position[1]=y+1.64;   //    1.72;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval;
				electrodes[kk].stop_msec=(kk+1)*interval-50;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				
				electrodes[kk].position[0]=x+0.88;    //0.48;
				electrodes[kk].position[1]=y+0.48;        //0.88;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval;
				electrodes[kk].stop_msec=(kk+1)*interval-50;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				
				electrodes[kk].position[0]=x+1.28;    //1.72;
				electrodes[kk].position[1]=y+0.49;         //.5;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval;
				electrodes[kk].stop_msec=(kk+1)*interval-50;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				*/
					
				
				stim_group("M1p5(L56)", electrodes, t, trial_duration_msec, exc_output);
				
				
				
			}

			
			
			if( sec >= V1_stimulation_start_time_sec && sec <= V1_stimulation_end_time_sec && group_data.exists("V1TCs") ){
				
				float distance = 0.2;
				//float xo[]={.2, .2, 1,   1, 1.8,1.8, .6, .6, 1.4, 1.4,.2, .2, 1, 1, 1.8,1.8, .6, .6,  1.4, 1.4, .2, .2,  1,  1, 1};
				//float yo[]={.2, .2, .2, .2, .6, .6,  .6, .6, .6, .6,   1, 1,  1,1,  1,   1,   1.4,1.4,1.4, 1.4, 1.8,1.8,1.8,1.8,1.8};

				//float distance[]={0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4,0.2,0.4};
				float stim_current = 1000;
				float start_offset=50;
				float end_offset=50;
				g = group_data["V1TCs"];
				
				float x= g.x0;
				float y= g.y0;
				float width_mm = g.area_width_mm;
				vector<electrode> electrodes(n_patterns);
				
				for(kk=0;kk<n_patterns;kk++){
					//electrodes[kk].position[0]=x+xo[permu[kk]-1]; 
					//electrodes[kk].position[1]=y+yo[permu[kk]-1];
					electrodes[kk].position[0]=x+xo[kk]; 
					electrodes[kk].position[1]=y+yo[kk]; 
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=kk*interval;
					electrodes[kk].stop_msec=(kk+1)*interval-50;
					//electrodes[kk].dist=distance[kk];
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				}
				
				
				
				/*
				kk=0;
				electrodes[kk].position[0]=x+0.56;
				electrodes[kk].position[1]=y+1.72;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval+start_offset;
				electrodes[kk].stop_msec=(kk+1)*interval+end_offset;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				
				electrodes[kk].position[0]=x+0.48;
				electrodes[kk].position[1]=y+0.88;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval+start_offset;
				electrodes[kk].stop_msec=(kk+1)*interval+end_offset;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				
				electrodes[kk].position[0]=x+1.72;
				electrodes[kk].position[1]=y+.5;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval+start_offset;
				electrodes[kk].stop_msec=(kk+1)*interval+end_offset;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				
				electrodes[kk].position[0]=x+1.68;
				electrodes[kk].position[1]=y+1.72;
				electrodes[kk].position[2]=0;
				electrodes[kk].start_msec=kk*interval+start_offset;
				electrodes[kk].stop_msec=(kk+1)*interval+end_offset;
				electrodes[kk].dist=distance;
				electrodes[kk].stim_current = stim_current;
				kk++;
				*/
				
				
				stim_group("V1TCs", electrodes, t, trial_duration_msec, exc_output);
				
				
				
			}
			
			
			

			//********************************************************
			if( sec >= S1_stimulation_start_time_sec && sec <= S1_stimulation_end_time_sec && group_data.exists("S1p23") ){
/*
				g = group_data["S1TCs"];

				float x= g.x0;
				float y= g.y0;
				float width_mm = g.area_width_mm;

				vector<electrode> electrodes(n_electrodes);

				for (kk=0;kk<steps;kk++) {
					electrodes[kk].position[0]=0.5+kk*0.1;
					electrodes[kk].position[1]=4.5+kk*0.1;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=kk*interval;
					electrodes[kk].stop_msec=(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};
				
				for (kk=steps;kk<steps*2;kk++) {
					electrodes[kk].position[0]=1.5-(kk%steps)*0.1;
					electrodes[kk].position[1]=5.5-(kk%steps)*0.1;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=225+kk*interval;
					electrodes[kk].stop_msec=225+(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};
				
				
				for (kk=steps*2;kk<steps*3;kk++) {
					electrodes[kk].position[0]=1.5-(kk%steps)*0.1;
					electrodes[kk].position[1]=4.5+(kk%steps)*0.1;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=450+kk*interval;
					electrodes[kk].stop_msec=450+(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};
				
				for (kk=steps*3;kk<steps*4;kk++) {
					electrodes[kk].position[0]=0.5+(kk%steps)*0.1;
					electrodes[kk].position[1]=5.5-(kk%steps)*0.1;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=225*3+kk*interval;
					electrodes[kk].stop_msec=225*3+(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};
				
				for (kk=steps*4;kk<steps*5;kk++) {
					electrodes[kk].position[0]=1.0;
					electrodes[kk].position[1]=4.5+(kk%steps)*0.1;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=225*4+kk*interval;
					electrodes[kk].stop_msec=225*4+(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};
				
				for (kk=steps*5;kk<steps*6;kk++) {
					electrodes[kk].position[0]=1.0;
					electrodes[kk].position[1]=5.5-(kk%steps)*0.1;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=225*5+kk*interval;
					electrodes[kk].stop_msec=225*5+(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};
				
				for (kk=steps*6;kk<steps*7;kk++) {
					electrodes[kk].position[0]=0.5+(kk%steps)*0.1;
					electrodes[kk].position[1]=5.0;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=225*6+kk*interval;
					electrodes[kk].stop_msec=225*6+(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};
				
				for (kk=steps*7;kk<steps*8;kk++) {
					electrodes[kk].position[0]=1.5-(kk%steps)*0.1;
					electrodes[kk].position[1]=5.0;
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=225*7+kk*interval;
					electrodes[kk].stop_msec=225*7+(kk+1)*interval;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				};

				stim_group("S1TCs", electrodes, t, trial_duration_msec, exc_output);
			
			*/	
			
			float stim_current=800;
			const bool use_gabor = false;
			if( use_gabor ){
				// Use gabor functions to test the mapping capabilities of the network.
				g = group_data["S1TCs"];
				
				float cycles = 1.0;
				float phase_percent = 0.0f;
			
				int pattern = (t/interval)%n_patterns;
				gabor( exc_output+g.first_neuron_id, g.rows, (float) g.rows/2, (float) g.cols/2, (float)pattern/(float)n_patterns*pi, cycles, phase_percent, stim_current);
				// rectify
				for(int i = g.first_neuron_id;i<g.last_neuron_id;i++){
					exc_output[i] = max(exc_output[i],0.0f);
				}
				
			}else{
				
			  //if( t% motor_update_interval_ms > 0 ){
				g = group_data["S1TCs"];
				gnum=g.last_neuron_id-g.first_neuron_id;
				pnum=(int)(gnum/6);

				float max_angle = pi; // 180 degrees is the max range of the joints.
				//float max_angle_vel=2*pi;
				float max_angle_vel=0.0;
				
			  	//if (myprocid == 0) cout << g.last_neuron_id << " " << g.first_neuron_id << endl;
				
				  // make the range bigger to make the patterns more separable.
				  float shoulder=2*shoulder_sensory;
				  float deltoid=2*deltoid_sensory;
				  float elbow=2*elbow_sensory;
				
				
				
				//for(int i = g.first_neuron_id; i <=g.last_neuron_id; i++){
				temp=(length(a,b,shoulder)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				int j=0;
				for (int i = g.first_neuron_id; i <=g.first_neuron_id+pnum/2; i++) {
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp-thresh);
					j++;
					//if (myprocid == 0) cout << i << ":" << exc_output[i] << "; "; 
				}
				//if(myprocid==0) cout << endl;
				
				
				temp=(length(a,b,max_angle-shoulder)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				j=0;
				for (int i = g.first_neuron_id+pnum/2; i <=g.first_neuron_id+pnum; i++) {
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp-thresh);
					j++;
				}
				
				// Calculate instantaneous velocity, dmuscle_length/djoint_angle, where joint angle is in radians per sec.
				// a should not equal b!
				temp=dlength(a,b,shoulder);
				temp2 = temp * max(0.0f,sdot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
			//  if (myprocid == 0) cout << temp2 << " "; 
				j=0;
				for (int i=g.first_neuron_id+pnum+1; i<=g.first_neuron_id+pnum+pnum/2;i++){
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp2-thresh);
					j++;
					//if (myprocid == 0) cout << i << ":" << exc_output[i] << "; "; 
				}
				//if(myprocid==0) cout << endl;
				
				temp2 = temp * max(0.0f,max_angle_vel-sdot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
			// if (myprocid == 0) cout << temp2 << endl; 
				j=0;
				for (int i=g.first_neuron_id+pnum+pnum/2+1; i<=g.first_neuron_id+2*pnum;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp2-thresh);
					j++;
				}
				
				
				// ********* deltoid
				
				temp=(length(a,b,deltoid)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				j=0;
				for (int i=g.first_neuron_id+2*pnum+1; i<=g.first_neuron_id+2*pnum+pnum/2;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp-thresh);
					j++;
				}
				
				temp=(length(a,b,max_angle-deltoid)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				j=0;
				for (int i=g.first_neuron_id+2*pnum+pnum/2+1; i<=g.first_neuron_id+3*pnum;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp-thresh);
					j++;
				}
				
				temp=dlength(a,b,deltoid);
				temp2 = temp * max(0.0f,ddot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				j=0;
				for (int i=g.first_neuron_id+3*pnum+1; i<=g.first_neuron_id+3*pnum+pnum/2;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp2-thresh);
					j++;
				}
				
				temp2 = temp * max(0.0f,max_angle_vel-ddot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				j=0;
				for (int i=g.first_neuron_id+3*pnum+pnum/2+1; i<=g.first_neuron_id+4*pnum;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp2-thresh);
					j++;
				}
				

				// *************** elbow
				
				temp=(length(a,b,elbow)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				j=0;
				for (int i=g.first_neuron_id+4*pnum+1; i<=g.first_neuron_id+4*pnum+pnum/2;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp-thresh);
					j++;
				}
				
				temp=(length(a,b,max_angle-elbow)-(a-b))/(2*b); // Convert angle to stretch receptor length and normalize to [0-1]
				j=0;
				for (int i=g.first_neuron_id+4*pnum+pnum/2+1; i<=g.first_neuron_id+5*pnum;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp-thresh);
					j++;
				}
				
				temp=dlength(a,b,elbow);
				temp2 = temp * max(0.0f,edot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				j=0;
				for (int i=g.first_neuron_id+5*pnum+1; i<=g.first_neuron_id+5*pnum+pnum/2;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp2-thresh);
					j++;
				}
				
				temp2 = temp * max(0.0f,max_angle_vel-edot);  // dmuscle_length/djoint_angle * djoint_angle/dt = dmuscle_length/dt.
				j=0;
				for (int i=g.first_neuron_id+5*pnum+pnum/2+1; i<g.first_neuron_id+6*pnum;i++)
				{
					float thresh = (float)j/(float)(pnum/2);
					exc_output[i]=stim_current*max(0.0f,temp2-thresh);
					j++;
				}
				 
				  // normalize 
				   float total = 0.0f;
				   for(int gnum=g.first_neuron_id;gnum<g.last_neuron_id;gnum++){
					   total += exc_output[gnum];
				   }
				  if( myprocid == 0)  cout << "TOTAL BEFORE NORM: " << total << " " << flush;

				  /*
				  total = 60000.0f/total;
				  for(int gnum=g.first_neuron_id;gnum<g.last_neuron_id;gnum++){
					  exc_output[gnum]*=total;
				  }

				  // DEBUG
				  total = 0.0f;
				  for(int gnum=g.first_neuron_id;gnum<g.last_neuron_id;gnum++){
					  total += exc_output[gnum];
				  }
				  if( myprocid == 0)  cout << " TOTAL STIMULATION: " << total << " " << shoulder << " " << deltoid << " " << elbow << " " << flush;
				   */
				   
				// DEBUG
				//for (int i=g.first_neuron_id+pnum+1; i<=g.first_neuron_id+pnum+pnum/2;i++){
				//	if (myprocid == 0) cout << i << ":" << exc_output[i] << "; "; 
				//}
				//if(myprocid==0) cout << endl;
				
			   }	
				
			}
			


			//******************  Motor babbling.  Stimulate the Layer V cells to get it going.
			
			
			if( sec >= M1_out_stimulation_start_time_sec && sec <= M1_out_stimulation_end_time_sec && group_data.exists("M1p5(L56)") ){

				/*
				vector<electrode> electrodes(n_electrodes);
				float center_x = 1.0;
				float center_y = 1.0;
				float radius = 0.6;
				for( int i = 0; i < n_electrodes; i++ ){
					
					float theta_rad = i*2*pi/n_electrodes;
					electrodes[i].position[0]=center_x + radius*cos(theta_rad);
					electrodes[i].position[1]=center_y + radius*sin(theta_rad);
					electrodes[i].position[2]=0;
					electrodes[i].start_msec=(int)((float) i*trial_duration_msec/n_electrodes-10);
					electrodes[i].stop_msec=(int)((float)(i+1)*trial_duration_msec/n_electrodes-1-40);
					electrodes[i].dist=distance;
					electrodes[i].stim_current = stim_current;
					
				}
				
				*/
				
				g = group_data["M1p5(L56)"];
				
				float stim_current = 2000.0;
				float distance = 0.2;
				
				float x= g.x0;
				float y= g.y0;
				float width_mm = g.area_width_mm;
				vector<electrode> electrodes(n_patterns);
				
				
				for(kk=0;kk<n_patterns;kk++){
					electrodes[kk].position[0]=x+xo[kk]; 
					electrodes[kk].position[1]=y+yo[kk]; 
					electrodes[kk].position[2]=0;
					electrodes[kk].start_msec=kk*interval;
					electrodes[kk].stop_msec=(kk+1)*interval-50;
					electrodes[kk].dist=distance;
					electrodes[kk].stim_current = stim_current;
				}
				
				
				stim_group("M1p5(L56)", electrodes, t, trial_duration_msec, exc_output);
				//stim_group("M1p5(L23)", electrodes, t, trial_duration_msec, exc_output);
				//stim_group("M1b5", electrodes, t, exc_output);  //Petersen & Sakman ,2001 discussion says that fewer inhib cells likely activated directly by microstim.
				//stim_group("M1nb5", electrodes, t, exc_output);

				
				
			}
			
			
			
			//******************* Part II.  Set the motor commands based on Layer V cell activity.
			// Only procid==0 has to deal with this.  No sense in having everyone do it redundantly.

			

			shoulder = 0.0f;
			deltoid = 0.0f; 
			elbow = 0.0f;			
			
			
			g = group_data["M1p5(L56)"];
			gnum=g.last_neuron_id-g.first_neuron_id;
			
			int window = 10;
			float tau = exp(-1.0/(float)window);  // 100 msec time constant; trying to match time constant of slow twitch muscles.
			
			if( myprocid == 0 ){
				shoulder = 0.0f;
				deltoid = 0.0f; 
				elbow = 0.0f;
				float rate_sum = 0.0f;
				// 1. Interpret activity
				int i = g.first_neuron_id;
				int votes = 0;
				for( int row = 0; row< g.rows; row++){
					for( int col = 0; col < g.cols; col++ ){

						mfr[i]= tau*mfr[i] + (1.0f-tau)*(spiked[i]?1.0:0.0)*1000 ; //This gives mean rate in Hz.
						
						if( mfr[i] > 10.0 ){  // only vote if you're serios about it.
							
							// Generate population vector which will determine instantaneous posture target.
							shoulder += mfr[i] * shoulder_lut_rad[row][col];
							deltoid  += mfr[i] * deltoid_lut_rad[row][col];
							elbow    += mfr[i] * elbow_lut_rad[row][col];
							rate_sum += mfr[i];
							votes++;
						}
						
						
						if(t%pattern_duration_msec == window && myprocid==0) cout <<fixed << setw(5) << setprecision(1)  << mfr[i] << " " ;
						
						
						i++;

					}
					if(t%pattern_duration_msec == window && myprocid==0) cout <<endl ;
				}
				if(t%pattern_duration_msec == window && myprocid==0) cout <<endl ;

				
				
/*				
				for(int i = g.first_neuron_id; i <=g.last_neuron_id; i++){
					count_fired += spiked[i] ? 1 : 0;
				}
				
				
				
				if( t%1000 == 999){
					cout << count_fired/(g.rows*g.cols) << endl;
					count_fired=0;
				}
*/				
				
				
				
				if( rate_sum > .00001 && votes >=10){
					shoulder *= 1.0f/rate_sum;
					deltoid *= 1.0f/rate_sum;
					elbow *= 1.0f/rate_sum;			
				}
				else{
					shoulder = home_pose_rad[0];
					deltoid = home_pose_rad[1];
					elbow = home_pose_rad[2];					
				}
				sdot= fabs(shoulder-shoulder_sensory)*max_velocity_rad_s* 0.5;
				ddot= fabs(deltoid-deltoid_sensory)*max_velocity_rad_s* 0.5;
				edot= fabs(elbow-elbow_sensory)*max_velocity_rad_s* 0.5;  // 10% of max velocity.
				
				fout << shoulder << ' ' << deltoid<<' ' << elbow << ' ';
				fout << shoulder_sensory << ' ' << deltoid_sensory<<' ' << elbow_sensory << endl;
				
				// DEBUG
				/*
				int row;
				int col = g.cols/2;
				if( t%1000 < 500) row = g.rows-1;
				if( t%1000 >= 500) row = 0;
				*/
					
			
					
					
				/*
				int row = myrand(0,g.rows-1);
				int col = myrand(0,g.cols-1);
				

				shoulder = shoulder_lut_rad[row][col];
				deltoid = deltoid_lut_rad[row][col];
				elbow = elbow_lut_rad[row][col];
				*/
				
				// 2. Move the arm.  
				// It would be nice if the null position was hanging down at its side.
				
				if( sec >= 0 )
				{ // We don't want to have the arm move to the zero position.
					const int motor_update_interval_ms = 250;
					const int motor_update_offset_ms = window;
					#ifdef APEX3
					//apex_client.set_arm_command( shoulder, deltoid, elbow, sdot, ddot, edot,t, motor_update_interval_ms, motor_update_offset_ms);
					#else
					apex.set_arm_command( shoulder, deltoid, elbow, sdot, ddot, edot,t, motor_update_interval_ms, motor_update_offset_ms);
					#endif
			
				}
				
			}
	
		}
	
/*	
		// update DA
		int daFired = 0;
		for (int i=0; i < neurons[ngDA].size(); i++)
			daFired += fired[neurons[ngDA][i]] ? 1 : 0;
		DA = min(4.0, DA+ 2*(double)daFired / (double)neurons[ngDA].size());
		//DA += (double)daFired / (double)neurons[ngDA].size();
		
		dahist << DA << endl;
*/		
	}; // interact()


	
	//******************************
	int get_sim_end_time_sec(){
		return sim_end_time_sec;
	};


//private:
	
	int sim_end_time_sec;
	
	ofstream fout, patternhist;
#ifdef APEX3
	//apex_robot< typename RobotType >  apex_client;
	apex_robot< ::nsi::thinklink::remote_robot<> >  apex_client;
#endif
		
#ifdef APEX2
	robot apex;
#endif
	int max_neuron_id;

	
	//vector<float> jointangles;
	groups group_data;

	int count_fired;
	
	int myprocid;
	
	vector<vector<float> > shoulder_lut_rad;
	vector<vector<float> > deltoid_lut_rad;
	vector<vector<float> > elbow_lut_rad;
	  
	int pattern_duration_msec;
	
}; // class world 

